package org.noote.libs.computervision.detection.circle;

import org.noote.libs.computervision.buffer.Buffer8I;

public class HoughDetection {
	public int[][] HoughCircles(Buffer8I src, int magnitudeThreshold, int houghThreshold, int minRadius, int maxRadius)
	{
		double maxR = maxRadius, minR = minRadius;

		int width = src.getWidth(), height = src.getHeight();

		/* Implement op here */
		double direction;
		double minA = 0., maxA = width;
		double minB = 0., maxB = width;
		double rangeA = maxA - minA, rangeB = maxB - minB, rangeR = maxR - minR;
		double rangeA_x_rangeB = rangeA * rangeB;
		int acummDimension = (int) (rangeA_x_rangeB);
		int accum[][] = new int[(int) rangeR + 1][];
		int gradient;
		double PIdiv2 = Math.PI / 2.0;
		double PIdiv2Neg = -PIdiv2;
		double PI_X_05 = 0.5 * Math.PI;

		for (int v = (int) maxR; v < height - (int) maxR; v++)
		{
			for (int u = (int) maxR; u < width - (int) maxR; u++)
			{
				// do Sobel operator
				double sy = (src.getPixel(u - 1, v) * -2
						+ src.getPixel(u + 1, v) * 2
						+ src.getPixel(u - 1, v - 1) * -1
						+ src.getPixel(u + 1, v - 1)
						+ src.getPixel(u - 1, v + 1) * -1 + src.getPixel(
						u + 1, v - 1)) / 8.0;
				double sx = (src.getPixel(u - 1, v - 1)
						+ src.getPixel(u, v - 1) * 2
						+ src.getPixel(u + 1, v - 1)
						+ src.getPixel(u - 1, v + 1) * -1
						+ src.getPixel(u, v + 1) * -2 + src.getPixel(
						u + 1, v - 1) * -1) / 8.0;
				// compute the gradient image from the original image
				gradient = (int) Math.sqrt(sx * sx + sy * sy);
				// compute the direction
				if (sy == 0)
				{
					if (sx == 0)
						direction = 0.;
					else
						direction = (sx > 0) ? PI_X_05 : -PI_X_05;
				}
				else if (sx == 0)
				{
					direction = 0.0;
				}
				else
				{
					direction = Math.atan(sy / sx);
				}
				direction += PIdiv2;
				// force direction to go from PI/2 to -PI/2
				if (direction > PIdiv2)
					direction -= Math.PI;
				if (direction < PIdiv2Neg)
					direction += Math.PI;

				// if the mag is bigger than threshold, increment accumulator
				// array
				if (gradient > magnitudeThreshold)
				{
					double theta = direction;
					for (double r = minR; r <= maxR; r += 1.)
					{
						double a = u - r * Math.cos(theta);
						double b = v - r * Math.sin(theta);
						try
						{
							if (accum[(int) (r - minR)] == null)
							{
								accum[(int) (r - minR)] = new int[acummDimension];
							}
							accum[(int) (r - minR)][(int) b * (int) rangeA
									+ (int) a]++;
						}
						catch (ArrayIndexOutOfBoundsException e)
						{
						}
					}
				}
			}
		}

		int thresh[][] = new int[(int) rangeR + 1][(int) (rangeA_x_rangeB)];
		// threshold the hough space; find the local maxima
		int numCirculos = 0;
		for (int r = 0; r <= rangeR; r++)
		{
			for (int x = 0; x < rangeA_x_rangeB; x++)
			{
				if (x % rangeA > 1 && x % rangeA < rangeA - 1 && x > rangeA
						&& x < rangeA_x_rangeB - rangeA
						&& accum[r][x] >= houghThreshold
						&& accum[r][x] > accum[r][x + 1]
						&& accum[r][x] > accum[r][x - 1]
						&& accum[r][x] > accum[r][x + (int) rangeA]
						&& accum[r][x] > accum[r][x - (int) rangeA]
						&& accum[r][x] > accum[r][x + (int) rangeA + 1]
						&& accum[r][x] > accum[r][x - (int) rangeA + 1]
						&& accum[r][x] > accum[r][x + (int) rangeA - 1]
						&& accum[r][x] > accum[r][x - (int) rangeA - 1])
				{
					thresh[r][x] = 255;
					// System.out.println("Punto=("+r+","+x+")<--255");
					numCirculos++;
				}
			}
		}

		if (numCirculos == 0)
			return null;

		int[][] cercles = new int[numCirculos][3];//[rayon, x, y]
		int indiceCirculos = 0;
		for (int r = (int) minR; r <= maxR; r++)
		{
			for (int x = 0; x < rangeA_x_rangeB; x++)
			{
				if (thresh[r - (int) minR][x] != 0)
				{
					// System.out.println("Circulo, radio="+r+"X="+(x%rangeA)+" Y="+(int)(x / rangeA));
					//circulos[indiceCirculos++] = new Circle(r,(int) (x % rangeA) - r, (int) (x / rangeA) - r);
					
					//rayon
					cercles[indiceCirculos][0] = r;
					//X centre
					cercles[indiceCirculos][1] = (int) (x % rangeA) - r;
					//Y centre
					cercles[indiceCirculos++][2] = (int) (x / rangeA) - r;
				}
			}
		}
		return cercles;//circulos;
	}
}
